##coding=utf-8
import sys,time,serial
sys.path.insert(0, ".")
import pywin.debugger as dbg

#import queue
import collections

import logging
from logging import handlers

def get_logger(logger_name, log_file, level=logging.INFO):
    formatter = logging.Formatter('%(asctime)s : %(message)s', "%Y-%m-%d %H:%M:%S")
    fileHandler = handlers.RotatingFileHandler(logfile,'w',2000,0)
    fileHandler.setFormatter(formatter)

    vlog = logging.getLogger(logger_name)
    vlog.setLevel(level)
    vlog.addHandler(fileHandler)

    return vlog


gAgv = None
SDOTYPE,PDOTYPE="a","z"

gCOM = sys.argv[1]
gIdx = eval(sys.argv[2])

from BitClass import bitwise_operator

#dbg.set_trace()
ser = serial.Serial(port=gCOM,stopbits = 1, bytesize = 8, parity = 'N', baudrate= 9600, timeout= 1)
#ser = serial.Serial(port="/dev/ttyUSB0",stopbits = 1, bytesize = 8, parity = 'N', baudrate= 9600, timeout= 1)

def LoraSendPDO(info):
    cmd = PDOTYPE+info+'\n'
    logger.info("TPDO:%s"%cmd)
    if (ser.isOpen()):
        ser.write(cmd)

def LoraReadCmd():
        c = ser.readline()
        logger.info("Msg %s"%c)
        try:
          if c[0] == SDOTYPE or c[0] == PDOTYPE:
            #logger.info("Read Pkg%s"%c)
            return c
        except:
            return ''
        return ''
class AGV_Client(object):
    def __init__(self,AGV_id):
        self.name = "AGV%d"%AGV_id
        self.Task = []
        self.Status = {'CurNode':0,'NextNode':0,'SegProgess':1,'StopOrRun':0,'ManuOrAuto':0,'StopNodeOrSeg':0,'NotYaw':0,'NoBlockOrBlock':0,'CFGOK':0,'SysNormalOrAlarm':0}#from plc
        self.AGVState = 0#0初始化1待机2手动4自动8报警
        self.NetState = 0#0Init 1InitOK 
    def InitAction(self):
        if (ser.isOpen()):
            cmd='a%d\n'%gIdx#['a']+[gIdx]+['\n']
            ser.write(cmd)
    def InitOK(self):
        self.NetState = 1
    def Listen(self):
        c = LoraReadCmd()
        if len(c) == 0:
            return 0
        #logger.info("State %d"%self.AGVState)
        if c == "RECFG":
            return -1
        if len(c)>0 and c[0] == SDOTYPE and (int(c[1])==gIdx or int(c[1])==0):# and self.AGVState<2:#when init can receive sdo
            self.ProcessSDO(c)
        if len(c)>0 and c[0] == PDOTYPE and int(c[1])==0 and self.AGVState==4:#only when auto can receive pdo
            self.ProcessPDO(c)
        self.AGVState = 4## get mode from plc
        return 1

    def ProcessSDO(self,c):
        '''#mimic sdo'''
        logger.info("Receive SDO:%s"%str(c))
        self.InitOK()
        self.Task = map(lambda x:(int(x)),c.split(',')[1:])
        LoraSendPDO("%d,%d,%d,%d,%d"%(gIdx,self.Status['CurNode'],self.Status['NextNode'],self.Status['SegProgess'],42))

    def ProcessPDO(self,c):
        '''Receive PDO from AGV refresh AGV_STATUS'''
        logger.info("Receive PDO:%s"%str(c))
        LoraSendPDO("%d,%d,%d,%d,%d"%(gIdx,self.Status['CurNode'],self.Status['NextNode'],self.Status['SegProgess'],42))

    def Init(self):
        self.NetState = 0
        self.InitAction()

gAgv = AGV_Client(gIdx)


logfile = 'plcagv%d.log'%gIdx
logger = get_logger(logfile,logfile)
logger.setLevel(logging.DEBUG)

#fh=handlers.RotatingFileHandler(logfile,'a',200,0)
#formatter = logging.Formatter("%(asctime)s - %(filename)s[line:%(lineno)d] - %(levelname)s: %(message)s")
#fh.setFormatter(formatter)
#logger.addHandler(fh)


if __name__ == "__main__":
    splt = "----"*10
    print(splt)
    logger.info(splt)
    while 1:
        #myv ar = root.get_child(["0:Objects", "2:Main","2:AGV1_cmd"])
        #gAgv.NetState = 0
        gAgv.Init()
        time.sleep(3)
        cnt = 3
        cnt2 = 10
        while 1:
            time.sleep(0.2)
            l = gAgv.Listen()
            if l == -1:
                break
            if not l:
                cnt2 -= 1
                if cnt2 == 0:
                    print "OFFLine"
                    break
            else:
                cnt2 = 10
            if gAgv.NetState == 0:
                cnt -= 1
                if cnt == 0:
                    print "init failed"
                    break
    

    
